/**
 * IK_Cut_Seq.pde
 *
 * 2008, James Stone, www.manofstone.com
 *
 * Based on Reach 2. 
 * Based on code from Keith Peters (www.bit-101.com)
 * 
 */
import processing.serial.*;

// String portname = "/dev/tty.usbserial-A5002rKW"; // james arduino
 String portname = "/dev/tty.usbserial-A1001NQr"; // thomas' arduino, w/ feet
//  String portname = "/dev/tty.SerialPort-1"; // for testing
Serial port;  // Create object from Serial class


int numSegments = 3;
float[] x = new float[numSegments];
float[] y = new float[numSegments];
float[] angle = new float[numSegments];
float segLength = 12 * 10;
float knifeLength = 17.5 * 10;
// 17.5 cm to knife edge
float targetX, targetY;

void setup() {
  size(400, 400);
  smooth(); 
  strokeWeight(20.0);
  stroke(0, 100);
  x[x.length-1] = 20;     // Set base x-coordinate
  y[x.length-1] = height;  // Set base y-coordinate
  
    // init serial
  port = new Serial(this, portname, 9600);
  
  // font crap
    PFont fontA = loadFont("Ziggurat-HTF-Black-32.vlw");
  textFont(fontA, 12); 
}

// servo 0, rotation of base
String angle2Servo0(float a) { // a = angle in radians    
   int center = 2830; // centered rotation
   int c90 = 4520; // 90deg to the right, clockwise
   int cc90 = 1140; // 90deg to the left, counter clockwise, calculated
   int value = center;  // if below is not met, a = 0 exactly
   if (a > 0) { // clockwise + angles
      value = (int)((c90 - center) / 90 * (degrees(a)) + center);
   } else if (a < 0) { // cc - angles
     value = (int)(center - (center - cc90) / 90 * abs((degrees(a))));
   }
  return "s0" + nf(value, 4); 
}

// servo 1, elevation of 1st arm segment from base, shoulder
String angle2Servo1(float a) { // a = angle in radians    
   int center = 1700; // parallel with the ground plane
  // int c90 = 3337; // perpendicular to the ground plane * old value, was working, angle changed to 3k, unknown reason
   int c90 = 3000; // perpendicular to the ground plane
   int value = center;  // if below is not met, a = 0 exactly
      value = (int)((c90 - center) / 90 * (degrees(a)) + center);
  return "s1" + nf(value, 4); 
}

// servo 2, elevation of 2nd arm segment from base, elbow
String angle2Servo2(float a) { // a = angle in radians    
   int c90 = 3400; // 90deg to the right, clockwise
   int center = c90 - (5190 - c90); // centered rotation
   int cc90 = center - (c90 - center); // 90deg to the left, counter clockwise, calculated
   int value = center;  // if below is not met, a = 0 exactly
   if (a > 0) { // clockwise + angles
      value = (int)(center - (center - c90) / 90 * (degrees(a)));
   }
  return "s2" + nf(value, 4); 
}

// servo 2, elevation of 2nd arm segment from base, elbow
String angle2Servo3(float a) { // a = angle in radians    
   int center = 3140; // centered rotation
   int c90 = 5030; // 90deg to the right, clockwise
   int cc90 = 1250; // 90deg to the left, counter clockwise, calculated
   int value = center;  // if below is not met, a = 0 exactly
   if (a > 0) { // clockwise + angles
      value = (int)((c90 - center) / 90 * (degrees(a)) + center);
   } else if (a < 0) { // cc - angles
     value = (int)(center - (center - cc90) / 90 * abs((degrees(a))));
   }
  return "s3" + nf(value, 4); 
}

float robX = 150;  // for moving arm specically
float robY = 332;
int baseRot = 3000;

void draw() {
	background(226);
	fill(0);
	if (millis() % 1 == 0) {
		reachSegment(0, robX, robY);
		for(int i=1; i<numSegments; i++) {
			reachSegment(i, targetX, targetY);
		}
		for(int i=x.length-1; i>=1; i--) {
			positionSegment(i, i-1);  
		} 
		for(int i=0; i<x.length; i++) {
			segment(x[i], y[i], angle[i], (i+1)*2); 
		}
	}
	text("robX =" + robX , 200, 100);
	text("robY =" + robY , 200, 125);
}
	
void calcIK() {
	reachSegment(0, robX, robY);
	for(int i=1; i<numSegments; i++) {
		reachSegment(i, targetX, targetY);
	}
	for(int i=x.length-1; i>=1; i--) {
		positionSegment(i, i-1);  
	} 
	for(int i=0; i<x.length; i++) {
		segment(x[i], y[i], angle[i], (i+1)*2); 
	}
}

void moveArm() {
	port.write(angle2Servo1(-angle[2]));
	port.write(angle2Servo2(abs(angle[2]) + angle[1]));
	port.write(angle2Servo3(angle[1]));
	port.write("s0" + nf(baseRot, 4)); // rotate base
}

void keyReleased() {
  switch (key) {
	case '1': // seq #1
		port.write("s43000"); // default servos
		robX = 187;
		robY = 236;
		calcIK();
		moveArm();
		port.write("v10000");
		delay(1000);
		robX = 220;
		calcIK();
		moveArm();
		delay(1000);
		port.write("v10000");
		delay(1000);
		robY = 300;
		robX = 220;
		calcIK();
		moveArm();
		delay(4000);
		port.write("v00000");
		delay(500);
		robX = 187;
		robY = 236;
		calcIK();
		moveArm();
		delay(1000);
  		robX = 150;
  		robY = 332;
		calcIK();
		moveArm();
		break;
  	case 'p': // park arm
  		robX = 150;
  		robY = 332;
  		break;
    case 'm': // move arm, send serial
		port.write("s43000"); // default servos
		port.write(angle2Servo1(-angle[2]));
		port.write(angle2Servo2(abs(angle[2]) + angle[1]));
		port.write(angle2Servo3(angle[1]));
		port.write("s0" + nf(baseRot, 4)); // rotate base
		break;
	case 's': //starting point
		robX = 187;
		robY = 236;
		break;
	case 'o': // open clamps
		port.write("c11000c21000c31000c41000"); // default servos
		break;
	case 'c': // close clamps
		port.write("c10000c20000c30000c40000"); // default servos
		break;
	case 'v':
		port.write("v10000"); // default servos
		break;
	case 'q':
		port.write("v00000"); // default servos
		break;
	case '+':
		baseRot += 10;
		break;
	case '-':
		baseRot -= 10;
		break;
	case CODED:
		switch(keyCode){
			case UP:
				robY--;
				break;
			case DOWN:
				robY++;
				break;
			case RIGHT:
				robX++;
				break;
			case LEFT:
				robX--;
				break;
		}
		break;
	default:
  		robX = 150; // park
  		robY = 216;
  		break;
  }
}

void positionSegment(int a, int b) {
  x[b] = x[a] + cos(angle[a]) * segLength;
  y[b] = y[a] + sin(angle[a]) * segLength; 
}

void reachSegment(int i, float xin, float yin) {
  float dx = xin - x[i];
  float dy = yin - y[i];
  if (i == 0) {
    angle[i] = 0;  
  } else {
    angle[i] = atan2(dy, dx);  
  }
  targetX = xin - cos(angle[i]) * segLength;
  targetY = yin - sin(angle[i]) * segLength;
}

void segment(float x, float y, float a, float sw) {
  strokeWeight(sw);
  pushMatrix();
  translate(x, y);
  rotate(a);
  line(0, 0, segLength, 0);
  popMatrix();
}
